#include <stdlib.h>
#include "avr_compiler.h"
#include "uart.h"
#include <stdio.h>
#include "pmd_control.h"
#include <util/delay.h>
/*
typedef struct
{
	//UART module pmd is located on
	USART_data_t * uart;
	volatile uint8_t data_in[20];
	volatile uint8_t data_out[20];
	volatile uint8_t command;
	volatile uint8_t data_in_count;
	volatile uint8_t data_out_count;

} PMD_Control_t;
*/
void pmd_no_operation(PMD_Control_t * pmd){
	uint8_t string[4] = {0,0,0,0};
	int i;
	for(i=0;i<4;i++){
		sendchar(&(pmd->uart), string[i]);
	}
}

	
//Will start at specified baudrate, but will always leave at 460800 baud
void pmd_init(PMD_Control_t * pmd, USART_t * uart, int baud){

	if(uart == &USARTC0){
		PORTC.DIR |= 0x08;
		uartInitiate(&(pmd->uart), &USARTC0, baud);
	}

	pmd->command = 0;
	pmd->data_in_count = 0;
	pmd->data_available = 0;
	pmd->error = 0;
	
}
void pmd_set_baud(PMD_Control_t * pmd, int baud){
	if(baud == 4608){
		set_serial_port_mode(pmd, SERIAL_PORT_4608);
		_delay_ms(2000);
		USART_Baudrate_Set(&USARTC0, 40, -6);
		_delay_ms(2000);
	}
}

void set_serial_port_mode(PMD_Control_t * pmd, uint16_t value){
	int i;
	uint8_t string[] = {0, 0, 0, SET_SERIAL_PORT_MODE, (uint8_t)(value >> 8), (uint8_t)(value & 0xff)};
	string[1] = generate_checksum(string, 6);
	for(i=0;i<6;i++){
		sendchar(&(pmd->uart), string[i]);
	}
	pmd->command = 0;
pmd->data_in_count = 0;

}



void set_pwm_frequency(PMD_Control_t * pmd, int16_t value){
	int i;
	uint8_t string[] = {0, 0, 0, SET_PWM_FREQUENCY, (uint8_t)(value >> 8), (uint8_t)(value & 0xff)};
	string[1] = generate_checksum(string, 6);
	for(i=0;i<6;i++){
		sendchar(&(pmd->uart), string[i]);
	}
	pmd->command = 0;
pmd->data_in_count = 0;

}

void set_velocity_feedback_source(PMD_Control_t * pmd, uint8_t value){
	int i;
	uint8_t string[] = {0, 0, 0, SET_VELOCITY_FEEDBACK_SOURCE, 0, value};
	string[1] = generate_checksum(string, 6);
	for(i=0;i<6;i++){
		sendchar(&(pmd->uart), string[i]);
	}
	pmd->command = 0;
	pmd->data_in_count = 0;
}

void set_velocity_scalar(PMD_Control_t * pmd, uint16_t velocity_scalar)
{
	int i;
	uint8_t string[] = {0, 0, 0, SET_VELOCITY_SCALAR, (uint8_t)(velocity_scalar>>8), (uint8_t)velocity_scalar};
	string[1] = generate_checksum(string, 6);
	for(i=0;i<6;i++)
	{
		sendchar(&(pmd->uart), string[i]);
	}
	pmd->command = 0;
	pmd->data_in_count = 0;
}


void set_analog_offset(PMD_Control_t * pmd, uint8_t target,uint16_t value)
{
	int i;
	uint8_t string[] = {0, 0, 0, SET_ANALOG_OFFSET, 0, (uint8_t)target, (uint8_t)(value >> 8), (uint8_t)(value & 0xff)};
	string[1] = generate_checksum(string, 8);
	for(i=0;i<8;i++)
	{
		sendchar(&(pmd->uart), string[i]);
	}
	pmd->command = 0;
	pmd->data_in_count = 0;
}



void set_error_limit(PMD_Control_t * pmd, uint16_t value){
	int i;
	uint8_t string[] = {0, 0, 0, SET_MOTION_ERROR_LIMIT, 0, 0, (uint8_t)(value>>8), (uint8_t)(value & 0xff)};
	string[1] = generate_checksum(string, 8);
	for(i=0;i<8;i++)
	{
		sendchar(&(pmd->uart), string[i]);
	}
	pmd->command = 0;
	pmd->data_in_count = 0;
}


void set_acceleration(PMD_Control_t * pmd, uint32_t acceleration)
{
int i;
	uint8_t string[] = {0, 0, 0, SET_ACCELERATION, (uint8_t)(acceleration >> 24), (uint8_t)(acceleration >> 16),(uint8_t)(acceleration >> 8), (uint8_t)(acceleration & 0xff)};
	string[1] = generate_checksum(string, 8);
	for(i=0;i<8;i++){
		sendchar(&(pmd->uart), string[i]);
	}
	pmd->command = 0;
	pmd->data_in_count = 0;
	
}

void set_loop_gain(PMD_Control_t * pmd, uint8_t loop_term, uint32_t value)
{
	int i;
	uint8_t string[] = {0, 0, 0, SET_LOOP_GAIN, 0, loop_term,(uint8_t)(value >> 24), (uint8_t)(value >> 16), (uint8_t)(value >> 8), (uint8_t)(value && 0xff)};
	string[1] = generate_checksum(string, 10);
	for(i=0;i<10;i++){
		sendchar(&(pmd->uart), string[i]);
	}
	pmd->command = 0;
	pmd->data_in_count = 0;

	
}

void set_command_source(PMD_Control_t * pmd, uint8_t value){
	int i;
	uint8_t string[] = {0, 0, 0, SET_COMMAND_SOURCE, 0, value};
	string[1] = generate_checksum(string, 6);
	for(i=0;i<6;i++){
		sendchar(&(pmd->uart), string[i]);
	}
	pmd->command = 0;
	pmd->data_in_count = 0;

}

void set_pwm_limit(PMD_Control_t * pmd, int16_t value){
	int i;
	uint8_t string[] = {0, 0, 0, SET_PWM_LIMIT, (uint8_t)(value >> 8), (uint8_t)(value & 0xff)};
	string[1] = generate_checksum(string, 6);
	for(i=0;i<6;i++){
		sendchar(&(pmd->uart), string[i]);
	}
	pmd->command = 0;
pmd->data_in_count = 0;

}
void set_dead_time(PMD_Control_t * pmd, int16_t value){
	int i;
	uint8_t string[] = {0, 0, 0, SET_DEAD_TIME, (uint8_t)(value >> 8), (uint8_t)(value & 0xff)};
	string[1] = generate_checksum(string, 6);
	for(i=0;i<6;i++){
		sendchar(&(pmd->uart), string[i]);
	}
	pmd->command = 0;
	pmd->data_in_count = 0;

}



void set_motor_limit(PMD_Control_t * pmd, int16_t value){
	int i;
	uint8_t string[] = {0, 0, 0, SET_MOTOR_LIMIT, (uint8_t)(value >> 8), (uint8_t)(value & 0xff)};
	string[1] = generate_checksum(string, 6);
	for(i=0;i<6;i++){
		sendchar(&(pmd->uart), string[i]);
	}
	pmd->command = 0;
pmd->data_in_count = 0;

}

void set_motor_mode(PMD_Control_t * pmd, uint8_t value){
	int i;
	uint8_t string[] = {0, 0, 0, SET_MOTOR_MODE, 0, value};
	string[1] = generate_checksum(string, 6);
	for(i=0;i<6;i++){
		sendchar(&(pmd->uart), string[i]);
	}
	pmd->command = 0;
pmd->data_in_count = 0;

}

void set_commutation_mode(PMD_Control_t * pmd, uint8_t value){
	int i;
	uint8_t string[] = {0, 0, 0, SET_COMMUTATION_MODE, 0, value};
	string[1] = generate_checksum(string, 6);
	for(i=0;i<6;i++){
		sendchar(&(pmd->uart), string[i]);
	}
	pmd->command = 0;
	pmd->data_in_count = 0;

}

void set_motor_command(PMD_Control_t * pmd, int16_t value){
	int i;
	uint8_t string[] = {0, 0, 0, SET_MOTOR_COMMAND, (uint8_t)(value >> 8), (uint8_t)(value & 0xff)};
	string[1] = generate_checksum(string, 6);
	for(i=0;i<6;i++){
		sendchar(&(pmd->uart), string[i]);
	}
	pmd->command = 0;
	pmd->data_in_count = 0;

}
void set_phase_counts(PMD_Control_t * pmd, uint16_t value){
	int i;
	uint8_t string[] = {0, 0, 0, SET_PHASE_COUNTS, (uint8_t)(value >> 8), (uint8_t)(value & 0xff)};
	string[1] = generate_checksum(string, 6);
	for(i=0;i<6;i++){
		sendchar(&(pmd->uart), string[i]);
	}
	pmd->command = 0;
	pmd->data_in_count = 0;

}
void set_phase_correction(PMD_Control_t * pmd, uint16_t value){
	int i;
	uint8_t string[] = {0, 0, 0, SET_PHASE_CORRECTION_MODE, (uint8_t)(value >> 8), (uint8_t)(value & 0xff)};
	string[1] = generate_checksum(string, 6);
	for(i=0;i<6;i++){
		sendchar(&(pmd->uart), string[i]);
	}
	pmd->command = 0;
	pmd->data_in_count = 0;

}

void set_signal_sense(PMD_Control_t * pmd, uint16_t value){
	int i;
	uint8_t string[] = {0, 0, 0, SET_SIGNAL_SENSE, (uint8_t)(value >> 8), (uint8_t)(value & 0xff)};
	string[1] = generate_checksum(string, 6);
	for(i=0;i<6;i++){
		sendchar(&(pmd->uart), string[i]);
	}
	pmd->command = 0;
	pmd->data_in_count = 0;

}
void set_pwm_sense(PMD_Control_t * pmd, uint8_t value){
	int i;
	uint8_t string[] = {0, 0, 0, SET_PWM_SENSE, 0, value};
	string[1] = generate_checksum(string, 6);
	for(i=0;i<6;i++){
		sendchar(&(pmd->uart), string[i]);
	}
	pmd->command = 0;
	pmd->data_in_count = 0;

}
void set_loop_mode(PMD_Control_t * pmd, uint8_t value){
	int i;
	uint8_t string[] = {0, 0, 0, SET_LOOP_MODE, 0, value};
	string[1] = generate_checksum(string, 6);
	for(i=0;i<6;i++){
		sendchar(&(pmd->uart), string[i]);
	}
	pmd->command = 0;
	pmd->data_in_count = 0;

}
void set_condition_mask(PMD_Control_t * pmd, uint16_t value, uint16_t value1){
	int i;
	uint8_t string[] = {0, 0, 0, SET_CONDITION_MASK, (uint8_t)(value >> 8), (uint8_t)(value & 0xff),(uint8_t)(value >> 8), (uint8_t)(value & 0xff)};
	string[1] = generate_checksum(string, 8);
	for(i=0;i<8;i++){
		sendchar(&(pmd->uart), string[i]);
	}
	pmd->command = 0;
	pmd->data_in_count = 0;

}
void set_velocity(PMD_Control_t * pmd, int32_t value){
	int i;
	uint8_t string[] = {0, 0, 0, SET_VELOCITY, (uint8_t)(value >> 24), (uint8_t)(value >> 16),(uint8_t)(value >> 8), (uint8_t)(value & 0xff)};
	string[1] = generate_checksum(string, 8);
	for(i=0;i<8;i++){
		sendchar(&(pmd->uart), string[i]);
	}
	pmd->command = 0;
	pmd->data_in_count = 0;

}



void pmd_reset(PMD_Control_t * pmd){
	int i;
	uint8_t string[] = {0, 0, 0, PMD_RESET};
	string[1] = generate_checksum(string, 4);
	for(i=0;i<4;i++){
		sendchar(&(pmd->uart), string[i]);
	}
	pmd->command = PMD_RESET;
pmd->data_in_count = 0;

}

void get_motor_command(PMD_Control_t * pmd){
	int i;
	uint8_t string[] = {0, 0, 0, GET_MOTOR_COMMAND};

	pmd->data_in_count = 0;
	string[1] = generate_checksum(string, 4);
	for(i=0;i<4;i++){
		sendchar(&(pmd->uart), string[i]);
	}
	pmd->command = GET_MOTOR_COMMAND;
pmd->data_in_count = 0;


}
void get_event_status(PMD_Control_t * pmd){
	int i;
	uint8_t string[] = {0, 0, 0, GET_EVENT_STATUS};

	pmd->data_in_count = 0;
	string[1] = generate_checksum(string, 4);
	for(i=0;i<4;i++){
		sendchar(&(pmd->uart), string[i]);
	}
	pmd->command = FOUR_BYTE;
	pmd->data_in_count = 0;


}


void get_dead_time(PMD_Control_t * pmd){
	int i;
	uint8_t string[] = {0, 0, 0, GET_DEAD_TIME};

	pmd->data_in_count = 0;
	string[1] = generate_checksum(string, 4);
	for(i=0;i<4;i++){
		sendchar(&(pmd->uart), string[i]);
	}
	pmd->command = GET_DEAD_TIME;
pmd->data_in_count = 0;


}
void get_pwm_limit(PMD_Control_t * pmd){
	int i;
	uint8_t string[] = {0, 0, 0, GET_PWM_LIMIT};

	pmd->data_in_count = 0;
	string[1] = generate_checksum(string, 4);
	for(i=0;i<4;i++){
		sendchar(&(pmd->uart), string[i]);
	}
	pmd->command = GET_PWM_LIMIT;
pmd->data_in_count = 0;


}

void get_activity_status(PMD_Control_t * pmd){
	int i;
	uint8_t string[] = {0, 0, 0, GET_ACTIVITY_STATUS};

	pmd->data_in_count = 0;
	string[1] = generate_checksum(string, 4);
	for(i=0;i<4;i++){
		sendchar(&(pmd->uart), string[i]);
	}
	pmd->command = GET_ACTIVITY_STATUS;
pmd->data_in_count = 0;


}



void get_commutation_mode(PMD_Control_t * pmd){
	int i;
	uint8_t string[] = {0, 0, 0, GET_COMMUTATION_MODE};

	pmd->data_in_count = 0;
	string[1] = generate_checksum(string, 4);
	for(i=0;i<4;i++){
		sendchar(&(pmd->uart), string[i]);
	}
	pmd->command = GET_COMMUTATION_MODE;
	pmd->data_in_count = 0;
}
void get_phase_counts(PMD_Control_t * pmd){
	int i;
	uint8_t string[] = {0, 0, 0, GET_PHASE_COUNTS};

	pmd->data_in_count = 0;
	string[1] = generate_checksum(string, 4);
	for(i=0;i<4;i++){
		sendchar(&(pmd->uart), string[i]);
	}
	pmd->command = GET_PHASE_COUNTS;
	pmd->data_in_count = 0;
}

void get_motor_limit(PMD_Control_t * pmd){
	int i;
	uint8_t string[] = {0, 0, 0, GET_MOTOR_LIMIT};

	pmd->data_in_count = 0;
	string[1] = generate_checksum(string, 4);
	for(i=0;i<4;i++){
		sendchar(&(pmd->uart), string[i]);
	}
	pmd->command = GET_MOTOR_LIMIT;
pmd->data_in_count = 0;


}
void get_pwm_frequency(PMD_Control_t * pmd){
	int i;
	uint8_t string[] = {0, 0, 0, GET_PWM_FREQUENCY};

	pmd->data_in_count = 0;
	string[1] = generate_checksum(string, 4);
	for(i=0;i<4;i++){
		sendchar(&(pmd->uart), string[i]);
	}
	pmd->command = GET_PWM_FREQUENCY;
pmd->data_in_count = 0;


}
void get_motor_mode(PMD_Control_t * pmd){
	int i;
	uint8_t string[] = {0, 0, 0, GET_MOTOR_MODE};
	string[1] = generate_checksum(string, 4);
	//uint8_t string[] = {0, 0x23, 0, 0xdd};
	for(i=0;i<4;i++){
		sendchar(&(pmd->uart), string[i]);
	}
	pmd->command = GET_MOTOR_MODE;
	pmd->data_in_count = 0;
}
void get_velocity(PMD_Control_t * pmd){
	int i;
	uint8_t string[] = {0, 0, 0, GET_VELOCITY};
	string[1] = generate_checksum(string, 4);
	//uint8_t string[] = {0, 0x23, 0, 0xdd};
	for(i=0;i<4;i++){
		sendchar(&(pmd->uart), string[i]);
	}
	pmd->command = GET_VELOCITY;
	pmd->data_in_count = 0;
}

void get_loop_mode(PMD_Control_t * pmd){
	int i;
	uint8_t string[] = {0, 0, 0, GET_LOOP_MODE};
	string[1] = generate_checksum(string, 4);
	//uint8_t string[] = {0, 0x23, 0, 0xdd};
	for(i=0;i<4;i++){
		sendchar(&(pmd->uart), string[i]);
	}
	pmd->command = GET_LOOP_MODE;
	pmd->data_in_count = 0;
}


void get_loop_error(PMD_Control_t * pmd, uint8_t value){
	int i;
	uint8_t string[] = {0, 0, 0, GET_LOOP_ERROR, 0, value};
	string[1] = generate_checksum(string, 6);
	//uint8_t string[] = {0, 0x23, 0, 0xdd};
	for(i=0;i<6;i++){
		sendchar(&(pmd->uart), string[i]);
	}
	pmd->command = SIX_BYTE;
	pmd->data_in_count = 0;
}

void get_loop_gain(PMD_Control_t * pmd, uint8_t value){
	int i;
	uint8_t string[] = {0, 0, 0, GET_LOOP_GAIN, 0, value};
	string[1] = generate_checksum(string, 6);
	//uint8_t string[] = {0, 0x23, 0, 0xdd};
	for(i=0;i<6;i++){
		sendchar(&(pmd->uart), string[i]);
	}
	pmd->command = SIX_BYTE;
	pmd->data_in_count = 0;
}



void get_position(PMD_Control_t * pmd){
	int i;
	uint8_t string[] = {0, 0, 0, 0x37};
	string[1] = generate_checksum(string, 4);
	for(i=0;i<4;i++){
		sendchar(&(pmd->uart), string[i]);
	}
	pmd->command = 0x37;
	pmd->data_in_count = 0;

}


void get_current_A(PMD_Control_t * pmd){
	int i;
	uint8_t string[] = {0, 0, 0, GET_ANALOG, 0, 2};
	string[1] = generate_checksum(string, 6);
	for(i=0;i<6;i++){
		sendchar(&(pmd->uart), string[i]);
	}
	pmd->command = GET_ANALOG;
	pmd->data_in_count = 0;

}

void get_current_B(PMD_Control_t * pmd){
	int i;
	uint8_t string[] = {0, 0, 0, GET_ANALOG, 0, 3};
	string[1] = generate_checksum(string, 6);
	for(i=0;i<6;i++){
		sendchar(&(pmd->uart), string[i]);
	}
	pmd->command = GET_ANALOG;
	pmd->data_in_count = 0;

}

void get_bus_voltage(PMD_Control_t * pmd){
	int i;
	uint8_t string[] = {0, 0, 0, GET_BUS_VOLTAGE};
	string[1] = generate_checksum(string, 4);
	for(i=0;i<4;i++){
		sendchar(&(pmd->uart), string[i]);
	}
	pmd->command = GET_BUS_VOLTAGE;
	pmd->data_in_count = 0;
}

/* old test function
void get_velocity(PMD_Control_t * pmd){
	int i;
	uint8_t string[] = {0, 0, 0, 0xAD};
	string[1] = generate_checksum(string, 4);
	for(i=0;i<4;i++){
		sendchar(&(pmd->uart), string[i]);
	}
	pmd->command = 0xAD;
	pmd->data_in_count = 0;


}
*/
uint8_t generate_checksum(volatile uint8_t * string,volatile uint8_t length){
	int i;
	uint8_t checksum = 0;
	for(i=0;i<length;i++){
		checksum += string[i];
	}
	checksum = ~checksum + 1;
	
	return checksum;
}
	
void pmd_in_handle(PMD_Control_t * pmd){
		USART_RXComplete(&(pmd->uart));
		pmd->data_in[pmd->data_in_count] = USART_RXBuffer_GetByte(&(pmd->uart));
	
	if(pmd->data_in[0] != 0){
		pmd->data_in_count ++;
		pmd->error = 1;
	
	}
	else if((pmd->data_in_count == 0) && (pmd->data_in[0] == 0)){
		pmd->data_in_count ++;
	}
	else if(pmd->data_in_count == 1){
		
		pmd->data_in_count ++;
		if(pmd->error == 1){
			pmd->command = 0;
		}
	
	}

	else if((pmd->command == GET_MOTOR_COMMAND) && (pmd->data_in_count > 1)){

		if(pmd->data_in_count == 2){
			pmd->data_in_count ++;
		}
		else{

			pmd->data_available = 1;
		}
	}
	else if((pmd->command == GET_MOTOR_MODE)  && (pmd->data_in_count > 1)){

		if(pmd->data_in_count == 2){
			pmd->data_in_count ++;
		}
		else{

			pmd->data_available = 1;
		}
	}
	else if((pmd->command == GET_MOTOR_LIMIT)  && (pmd->data_in_count > 1)){

		if(pmd->data_in_count == 2){
			pmd->data_in_count ++;
		}
		else{

			pmd->data_available = 1;
		}
	}
	else if((pmd->command == GET_PWM_FREQUENCY)  && (pmd->data_in_count > 1)){

		if(pmd->data_in_count == 2){
			pmd->data_in_count ++;
		}
		else{

			pmd->data_available = 1;
		}
	}
	else if((pmd->command == GET_COMMUTATION_MODE)  && (pmd->data_in_count > 1)){

		if(pmd->data_in_count == 2){
			pmd->data_in_count ++;
		}
		else{

			pmd->data_available = 1;
		}
	}
	else if((pmd->command == GET_PHASE_COUNTS)  && (pmd->data_in_count > 1)){

		if(pmd->data_in_count == 2){
			pmd->data_in_count ++;
		}
		else{

			pmd->data_available = 1;
		}
	}
	else if((pmd->command == GET_DEAD_TIME)  && (pmd->data_in_count > 1)){

		if(pmd->data_in_count == 2){
			pmd->data_in_count ++;
		}
		else{

			pmd->data_available = 1;
		}
	}
	else if((pmd->command == GET_PWM_LIMIT)  && (pmd->data_in_count > 1)){

		if(pmd->data_in_count == 2){
			pmd->data_in_count ++;
		}
		else{

			pmd->data_available = 1;
		}
	}
	else if((pmd->command == GET_ACTIVITY_STATUS)  && (pmd->data_in_count > 1)){

		if(pmd->data_in_count == 2){
			pmd->data_in_count ++;
		}
		else{

			pmd->data_available = 1;
		}
	}
	else if((pmd->command == GET_LOOP_MODE)  && (pmd->data_in_count > 1)){

		if(pmd->data_in_count == 2){
			pmd->data_in_count ++;
		}
		else{

			pmd->data_available = 1;
		}
	}

	else if((pmd->command == GET_BUS_VOLTAGE)  && (pmd->data_in_count > 1)){

		if(pmd->data_in_count == 2){
			pmd->data_in_count ++;
		}
		else{

			pmd->data_available = 1;
		}
	}

	else if((pmd->command == GET_VELOCITY)  && (pmd->data_in_count > 1)){

		if(pmd->data_in_count <= 4){
			pmd->data_in_count ++;
		}
		else{

			pmd->data_available = 1;
		}
	}

	else if((pmd->command == GET_ANALOG) && (pmd->data_in_count > 1)){

		if(pmd->data_in_count == 2){
			PORTA.OUT ^= 3;
			pmd->data_in_count ++;
		}
		else{
			pmd->data_available = 1;
		}
	}
	else if((pmd->command == FOUR_BYTE) && (pmd->data_in_count > 1)){

		if(pmd->data_in_count == 2){
			pmd->data_in_count ++;
		}
		else{
			pmd->data_available = 1;
		}
	}
	else if((pmd->command == SIX_BYTE) && (pmd->data_in_count > 1)){

		if(pmd->data_in_count <= 4){
			pmd->data_in_count ++;
		}
		else{
			pmd->data_available = 1;
		}
	}


	else{

	}
}
